# -*- coding: utf-8 -*-
'''
Created on 13.01.2020

@author: yu03
'''
import numpy as np
import os
import re
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
import glob
from mpl_toolkits.mplot3d import Axes3D
from PyUeye_Unified.Cross_200line_SAAC import folder_path, np_result_names, hor_index, ver_index, hor_lines, ver_lines
import sys
from FFT_Interpolation import FFT_interpolation_nonlinearity_compare
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter
from scipy.interpolate import interp1d



''' 
    Check Output File & Path
    Check Timestamp & Synchronization
''' 

output_path = folder_path + '\\'+ 'for Matthias'
output_path = output_path + '\\'  + 'corrected_097'
output_file = output_path +'\\'+'for Matthias' +'.raw'

name_encoder_f = folder_path + '\\' +  'F_en' + '.raw'
name_encoder_r = folder_path + '\\' +  'R_en' + '.raw'

name_SAAC_f_h = folder_path + '\\' +  'F_SAAC_H' + '.raw'
name_SAAC_f_v = folder_path + '\\' +  'F_SAAC_V' + '.raw'
name_SAAC_r_h = folder_path + '\\' +  'R_SAAC_H' + '.raw'
name_SAAC_r_v = folder_path + '\\' +  'R_SAAC_V' + '.raw'

 

def read_encoder(name_raw):
    print('Reading Data')
    with open(name_raw,'r') as fid:
        line='%'
        while line[0:3] != '%iP':
            line = fid.readline()
#             print(line)
        out_str = fid.readlines()
    iP, iH, iV, iA, cW,  EN_c,  EN_t,  EN_iP_H,  EN_iP_V,  EN_iP_A,  EN_H,  EN_V,  EN_A = [], [], [], [], [], [], [], [], [], [], [], [], []
    for line in out_str:
        a, b, c, d, e, f, g, h, i, j, k, l, m= line.strip().split(' ')
        EN_t.append(float(g)) # timestamp
        EN_H.append(float(k))
        EN_V.append(float(l))   
    EN_t = np.array(EN_t)
    EN_H = np.array(EN_H)
    EN_V = np.array(EN_V)
    return EN_t, EN_H, EN_V

def read_SAAC(name_raw):
    print('Reading Data')
    with open(name_raw,'r') as fid:
        line='%'
        while line[0:3] != '%iP':
            line = fid.readline()
#             print(line)
        out_str = fid.readlines()
    iP, iH, iV, iA, cW, AC_cCli, AC_tCli, AC_3, AC_003, AC_X, AC_Y = [], [], [], [], [], [], [], [], [], [], []
    for line in out_str:
        a, b, c, d, e, f, g, h, i, j, k= line.strip().split(' ')
        iP.append(a)
        iH.append(b)
        iV.append(c)
        iA.append(d)
        cW.append(e)
        AC_cCli.append(f)
        AC_tCli.append(float(g)) # timestamp
        AC_3.append(h)
        AC_003.append(i)
        AC_X.append(float(j))
        AC_Y.append(float(k))   
    AC_tCli = np.array(AC_tCli)
    AC_X = np.array(AC_X)
    AC_Y = np.array(AC_Y)
    return [AC_tCli, AC_X], iP, iH, iV

def Export_Data(file_name, header, out_str):
    print('Writing To File')
    with open(file_name,'w') as fid: ######################################################################################
        fid.writelines(header)
        fid.writelines(out_str)
    print('Finish Writing')
    return

i = 0
fs_cam = 25
hor_angle_avr_set = []
ver_angle_avr_set = []
hor_length_avr_set = []
ver_length_avr_set = []
time_set = []

''' 
    data reading
''' 
if 0:
    encoder_f_time, encoder_f_h, encoder_f_v = read_encoder(name_encoder_f)
    encoder_r_time, encoder_r_h, encoder_r_v = read_encoder(name_encoder_r)
    encoder_time = np.concatenate((encoder_f_time, encoder_r_time))
    encoder_h = np.concatenate((encoder_f_h, encoder_r_h))
    encoder_v = np.concatenate((encoder_f_v, encoder_r_v))
    encoder_time = encoder_time/1e3 #in second
    
    SAAC_f_h_time, SAAC_f_h = read_SAAC(name_SAAC_f_h)[0]
    SAAC_r_h_time, SAAC_r_h = read_SAAC(name_SAAC_r_h)[0]
    SAAC_h_time = np.concatenate((SAAC_f_h_time, SAAC_r_h_time))
    SAAC_h = np.concatenate((SAAC_f_h, SAAC_r_h))
    SAAC_h_time = SAAC_h_time/1e3 # in second
    
    SAAC_f_v_time, SAAC_f_v = read_SAAC(name_SAAC_f_v)[0]
    SAAC_r_v_time, SAAC_r_v = read_SAAC(name_SAAC_r_v)[0]
    SAAC_v_time = np.concatenate((SAAC_f_v_time, SAAC_r_v_time))
    SAAC_v = np.concatenate((SAAC_f_v, SAAC_r_v))
    SAAC_v_time = SAAC_v_time/1e3 # in second

if 1:
    SAAC_h_set, SAAC_v_set = [], []
    
    SAAC_raw_data = glob.glob(folder_path+'\*.raw')
    print(len(SAAC_raw_data))
    for f in SAAC_raw_data:
    #     print(i.strip().split('\\')[-1])
    #     print(i.strip().split('\\')[-1].strip().split('_'))
        data_name = f.strip().split('\\')[-1].strip().split('_')
    #     print(data_name[-1])
        if data_name[-1] == 'AH.raw':
            SAAC_h_set.append(f)
        elif data_name[-1] == 'AV.raw':
            SAAC_v_set.append(f)
            
    SAAC_h, SAAC_v, SAAC_h_time, SAAC_v_time = np.empty(1), np.empty(1), np.empty(1), np.empty(1)
    iP, iH, iV = np.empty(1), np.empty(1), np.empty(1)

    for i in range(len(SAAC_h_set)):
        SAAC_h_time = np.concatenate((SAAC_h_time, read_SAAC(SAAC_h_set[i])[0][0]))
        SAAC_h = np.concatenate((SAAC_h, read_SAAC(SAAC_h_set[i])[0][1]))
        iP = np.concatenate((iP, read_SAAC(SAAC_h_set[i])[1]))
        iH = np.concatenate((iH, read_SAAC(SAAC_h_set[i])[2]))
        
    for i in range(len(SAAC_v_set)):
        SAAC_v_time = np.concatenate((SAAC_v_time, read_SAAC(SAAC_v_set[i])[0][0]))
        SAAC_v = np.concatenate((SAAC_v, read_SAAC(SAAC_v_set[i])[0][1]))
        iV = np.concatenate((iV, read_SAAC(SAAC_h_set[i])[3]))
        
    SAAC_h_time = SAAC_h_time[1:]
    SAAC_h = SAAC_h[1:]
    SAAC_v_time = SAAC_v_time[1:]
    SAAC_v = SAAC_v[1:]
    iP = iP[1:]
    iH = iH[1:]
    iV = iV[1:]
    
    SAAC_h_time = SAAC_h_time/1e3 # in second
    SAAC_v_time = SAAC_v_time/1e3 # in second

''' 
    for all groups
''' 
for np_result in np_result_names:
    ''' 
        reading .npy file
    ''' 
    file_name = np_result.split('\\')[-1] ### x_lines.py
#     file_name = re.findall(r"openmode\\(.+).npy", np_result)[0]
    print(file_name)
    i += 1
    f = open(np_result, 'rb')
    f_size = os.fstat(f.fileno()).st_size
    ''' 
        put 4-D results in "lines"
    ''' 
    lines = []
    time_sequence = []
    while f.tell() < f_size:
        line = np.load(f, allow_pickle=True)
        lines.append(line)
    print('%i: %s, %.1fMB'%(i, file_name, f_size/1e6))
    time_sequence = lines[4]
    lines = np.array(lines[0:4])
    print(np.shape(lines), 'Channle, lines, frames:')
    frame_num = np.shape(lines)[2]
    line_num = np.shape(lines)[1]
    
#     print(np.shape(lines))
    hor_angle_set, ver_angle_set, hor_length_set, ver_length_set = lines
    time_set.append(time_sequence)
    ''' 
        making average
    ''' 
    averaged_results = []
    
#     index_line = 0
#     hor_angle_avr = hor_angle_set[index_line]
#     ver_angle_avr = ver_angle_set[index_line]
#     hor_length_avr = hor_length_set[index_line]
#     ver_length_avr = ver_length_set[index_line]
    
    hor_angle_avr = np.mean(hor_angle_set, axis=0)
    ver_angle_avr = np.mean(ver_angle_set, axis=0)
    hor_length_avr = np.mean(hor_length_set, axis=0)
    ver_length_avr = np.mean(ver_length_set, axis=0)
    
    hor_angle_avr_set.append(hor_angle_avr)
    ver_angle_avr_set.append(ver_angle_avr)
    hor_length_avr_set.append(hor_length_avr)
    ver_length_avr_set.append(ver_length_avr)

''' 
    data prosessing
''' 
if 1:
    time_set = np.array(time_set)
    time_set = np.hstack(time_set)  
#     time_set = (time_set - time_set[0]) * (1+2.326e-5) + time_set[0] + 0.2 ### for large
    time_set = (time_set - time_set[0]) * (1+8e-6) + time_set[0] + 0.2 ### for large
#    time_set = (time_set - time_set[0]) / (1+2.326e-5) + time_set[0] + 0.4 ### for fine
    
    hor_angle_avr_set = np.array(hor_angle_avr_set)
    ver_angle_avr_set = np.array(ver_angle_avr_set)
    hor_length_avr_set = np.array(hor_length_avr_set)
    ver_length_avr_set = np.array(ver_length_avr_set)
    
    hor_angle_avr_set = np.hstack(hor_angle_avr_set)  
    ver_angle_avr_set = np.hstack(ver_angle_avr_set)
    hor_length_avr_set = np.hstack(hor_length_avr_set)
    ver_length_avr_set = np.hstack(ver_length_avr_set)
    
    hor_angle_avr_set = hor_angle_avr_set * 0.20626479821012 # in arcsecond
    ver_angle_avr_set = ver_angle_avr_set * 0.20626479821012 # in arcsecond
    
    
    hor_angle_avr_set = (hor_angle_avr_set - hor_angle_avr_set[0]) * 0.97 + hor_angle_avr_set[0]
    ver_angle_avr_set = (ver_angle_avr_set - ver_angle_avr_set[0]) * 0.97 + ver_angle_avr_set[0]
    
#     encoder_h = encoder_h * -1 - 200
#     encoder_v = encoder_v * -1
    hor_angle_avr_set = hor_angle_avr_set + 257 + 2# + 5
    ver_angle_avr_set = ver_angle_avr_set * -1 - 196 -22
    
    hor_angle_avr_set = hor_angle_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:np.where(time_set > SAAC_h_time[-1])[0][0]]
    ver_angle_avr_set = ver_angle_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:np.where(time_set > SAAC_h_time[-1])[0][0]]
    hor_length_avr_set = hor_length_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:np.where(time_set > SAAC_h_time[-1])[0][0]]
    ver_length_avr_set = ver_length_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:np.where(time_set > SAAC_h_time[-1])[0][0]]
    time_set = time_set[np.where(time_set > SAAC_h_time[0])[0][0]:np.where(time_set > SAAC_h_time[-1])[0][0]]

#    hor_angle_avr_set = hor_angle_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:]
#    ver_angle_avr_set = ver_angle_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:]
#    hor_length_avr_set = hor_length_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:]
#    ver_length_avr_set = ver_length_avr_set[np.where(time_set > SAAC_h_time[0])[0][0]:]
#    time_set = time_set[np.where(time_set > SAAC_h_time[0])[0][0]:]
    
    print(len(time_set))
    print(len(SAAC_h))
#     print(len(encoder_h))
''' 
    data output
''' 
if 0:
    
    header = ['%\n'
              '% AC-Nachricht-GeräteInfo: 8 996 6 10 2011 300\n'
              '%iP iH iV iA cW  AC_cCli  AC_tCli  AC_3  AC_003  AC_X  AC_Y\n'
          ]

    
#     iP_f = read_SAAC(name_SAAC_f_h)[1]
#     iP_r = read_SAAC(name_SAAC_r_h)[1]
#     iP_f = np.array(iP_f)
#     iP_r = np.array(iP_r)
#     iP = np.concatenate((iP_f, iP_r))
# 
#     iH_f = read_SAAC(name_SAAC_f_h)[2]
#     iH_r = read_SAAC(name_SAAC_r_h)[2]
#     iH_f = np.array(iH_f)
#     iH_r = np.array(iH_r)
#     iH = np.concatenate((iH_f, iH_r))
# 
#     iV_f = read_SAAC(name_SAAC_f_v)[3]
#     iV_r = read_SAAC(name_SAAC_r_v)[3]
#     iV_f = np.array(iV_f)
#     iV_r = np.array(iV_r)
#     iV = np.concatenate((iV_f, iV_r))
    
    iP_set, iH_set, iV_set = [], [], []
    for time_stamp in time_set:
        iP_new = iP[np.where(SAAC_h_time < time_stamp)[0][-1]]
        iH_new = iH[np.where(SAAC_h_time < time_stamp)[0][-1]]
        iV_new = iV[np.where(SAAC_h_time < time_stamp)[0][-1]]
        iP_set.append(iP_new)
        iH_set.append(iH_new)
        iV_set.append(iV_new)
        if int(time_stamp)%100 == 0:
            print(iP_new, iH_new, iV_new)
    out_str = ['%s %s %s 1 1 %i %i 3 003 %.3f  %.3f\n' %(iP_set[i], iH_set[i], iV_set[i], i, time_set[i]*1e3, hor_angle_avr_set[i], ver_angle_avr_set[i]) for i in range(len(hor_angle_avr_set))]
    
    ''' 
        保存文件
    '''
    Export_Data(output_file, header, out_str)

''' 
    plotting
''' 
if 1:
    fig = plt.figure('Raw Data')
    plt.gcf().set_size_inches(18,9)
      
    ax1 = fig.add_subplot(2, 2, 1)
    ax2 = fig.add_subplot(2, 2, 2)
    ax3 = fig.add_subplot(2, 2, 3)
    ax4 = fig.add_subplot(2, 2, 4)
      
    ax1.plot(time_set, hor_angle_avr_set, color='black', marker=' ', markersize=1, label='horizontal angle')
    ax2.plot(time_set, hor_length_avr_set, color='black', marker=' ', markersize=1, label='horizontal length')
    ax3.plot(time_set, ver_angle_avr_set, color='black', label='vertical angle')
    ax4.plot(time_set, ver_length_avr_set, color='black', label='vertical length')
      
#     ax1.plot(encoder_time, encoder_h, color='blue', marker=' ', markersize=1, label='encoder')
    ax1.plot(SAAC_h_time, SAAC_h, color='red', marker=' ', markersize=1, label='SAAC')
#     ax3.plot(encoder_time, encoder_v, color='blue', marker=' ', markersize=1, label='encoder')
    ax3.plot(SAAC_v_time, SAAC_v, color='red', marker='', markersize=1, label='SAAC')
      
    ax1.title.set_text('Horizontal Tilt')
    ax2.title.set_text('Horizontal Length')
    ax3.title.set_text('Vertical Tilt')
    ax4.title.set_text('Vertical Length')
         
    ax1.set_ylabel('Horizontal Tilt \ acrsec')
    ax1.set_xlabel('Timestamp')
         
    ax2.set_ylabel('Horizontal Length \ nm')
    ax2.set_xlabel('Frame Num.')
         
    ax3.set_ylabel('Vertical Tilt \ arcsec')
    ax3.set_xlabel('Timestamp')
         
    ax4.set_ylabel('Vertical Length \ nm')
    ax4.set_xlabel('Frame Num.')
      
    ax1.grid(which='both', axis='both')
    ax2.grid(which='both', axis='both')
    ax3.grid(which='both', axis='both')
    ax4.grid(which='both', axis='both')
      
    ax1.legend()
    ax2.legend()
    ax3.legend()
    ax4.legend()
      
    plt.tight_layout()
#     plt.show()


'''
    Timestamp
'''
if 1:
    fig = plt.figure('Timestamp')
    plt.subplot(2,1,1)
    plt.plot(time_set, color='red', label='sensor')
    plt.plot(SAAC_h_time, color='black', label='SAAC_h')
    plt.plot(SAAC_v_time, color='green', label='SAAC_v')
    plt.grid()
    plt.legend()
    plt.subplot(2,1,2)
    plt.plot(time_set-(time_set[0]+(time_set[-1]-time_set[0])/len(time_set)*np.arange(len(time_set))), color='red', label='sensor')
    plt.plot(SAAC_h_time-(SAAC_h_time[0]+(SAAC_h_time[-1]-SAAC_h_time[0])/len(SAAC_h_time)*np.arange(len(SAAC_h_time))), color='black', label='SAAC_h')
    plt.plot(SAAC_v_time-(SAAC_v_time[0]+(SAAC_v_time[-1]-SAAC_v_time[0])/len(SAAC_v_time)*np.arange(len(SAAC_v_time))), color='green', label='SAAC_v')
    plt.legend()
    plt.grid()
    plt.tight_layout()

'''
    Repeatability
'''
if 0:
    
#     time_set = time_set - time_set[0]
#     SAAC_h_time = SAAC_h_time - SAAC_h_time[0]
#     SAAC_v_time = SAAC_v_time - SAAC_v_time[0]
    
    time_set_new = (time_set[-1] - time_set[0]) / len(time_set) * np.arange(len(time_set)) + time_set[0] ### 必须先线性化
    spline_hor_angle = interp1d(time_set_new, hor_angle_avr_set, kind='cubic', bounds_error=False, fill_value=0)
    spline_ver_angle = interp1d(time_set_new, ver_angle_avr_set, kind='cubic', bounds_error=False, fill_value=0)
    spline_hor_angle_new = spline_hor_angle(SAAC_h_time)
    spline_ver_angle_new = spline_ver_angle(SAAC_v_time)
    print(len(spline_hor_angle_new),len(spline_ver_angle_new))
    print(len(SAAC_h_time), len(SAAC_v_time))
    print(len(SAAC_h), len(SAAC_v))
    '''
        choose from wn_F_AH the first row (n=2,3,4) 
        &
        choose from wn_F_AV the first row (n=2,3,4) 
    '''
#     seperate_points = [1579189572857, 1579192327618, 1579195085707] ### for Z0_large_step

    seperate_points_hor = [1579276884473, 1579279658151, 1579282441345] ### for Z0_fine_step_1
    seperate_points_ver = [1579276884489, 1579279658214, 1579282441332] ### for Z0_fine_step_1
    
#     seperate_points_hor = [1579517553118, 1579520320353, 1579523094388] ### for Z0_fine_step_2
#     seperate_points_ver = [1579517553148, 1579520320353, 1579523094396] ### for Z0_fine_step_2
    
    seperate_points_hor = [k/1000 for k in seperate_points_hor]
    seperate_points_ver = [k/1000 for k in seperate_points_ver]
    
    SAAC_h_time_sub_1 = SAAC_h_time[:np.where(SAAC_h_time < seperate_points_hor[0])[0][-1]]
    SAAC_h_time_sub_2 = SAAC_h_time[np.where(SAAC_h_time > seperate_points_hor[0])[0][0]:np.where(SAAC_h_time < seperate_points_hor[1])[0][-1]]
    SAAC_h_time_sub_3 = SAAC_h_time[np.where(SAAC_h_time > seperate_points_hor[1])[0][0]:np.where(SAAC_h_time < seperate_points_hor[2])[0][-1]]
    SAAC_h_time_sub_4 = SAAC_h_time[np.where(SAAC_h_time > seperate_points_hor[2])[0][0]:]
    SAAC_h_time_sub_1 = SAAC_h_time_sub_1 - SAAC_h_time_sub_1[0]
    SAAC_h_time_sub_2 = SAAC_h_time_sub_2 - SAAC_h_time_sub_2[0]
    SAAC_h_time_sub_3 = SAAC_h_time_sub_3 - SAAC_h_time_sub_3[0]
    SAAC_h_time_sub_4 = SAAC_h_time_sub_4 - SAAC_h_time_sub_4[0]
    SAAC_h_time_sub_2 = (SAAC_h_time_sub_1[-1]-SAAC_h_time_sub_1[0])/(SAAC_h_time_sub_2[-1]-SAAC_h_time_sub_2[0]) * SAAC_h_time_sub_2 + SAAC_h_time_sub_1[0] 
    SAAC_h_time_sub_3 = (SAAC_h_time_sub_1[-1]-SAAC_h_time_sub_1[0])/(SAAC_h_time_sub_3[-1]-SAAC_h_time_sub_3[0]) * SAAC_h_time_sub_3 + SAAC_h_time_sub_1[0] 
    SAAC_h_time_sub_4 = (SAAC_h_time_sub_1[-1]-SAAC_h_time_sub_1[0])/(SAAC_h_time_sub_4[-1]-SAAC_h_time_sub_4[0]) * SAAC_h_time_sub_4 + SAAC_h_time_sub_1[0] 
    
    SAAC_v_time_sub_1 = SAAC_v_time[:np.where(SAAC_v_time < seperate_points_ver[0])[0][-1]]
    SAAC_v_time_sub_2 = SAAC_v_time[np.where(SAAC_v_time > seperate_points_ver[0])[0][0]:np.where(SAAC_v_time < seperate_points_ver[1])[0][-1]]
    SAAC_v_time_sub_3 = SAAC_v_time[np.where(SAAC_v_time > seperate_points_ver[1])[0][0]:np.where(SAAC_v_time < seperate_points_ver[2])[0][-1]]
    SAAC_v_time_sub_4 = SAAC_v_time[np.where(SAAC_v_time > seperate_points_ver[2])[0][0]:]
    
    SAAC_v_time_sub_1 = SAAC_v_time_sub_1 - SAAC_v_time_sub_1[0]
    SAAC_v_time_sub_2 = SAAC_v_time_sub_2 - SAAC_v_time_sub_2[0]
    SAAC_v_time_sub_3 = SAAC_v_time_sub_3 - SAAC_v_time_sub_3[0]
    SAAC_v_time_sub_4 = SAAC_v_time_sub_4 - SAAC_v_time_sub_4[0]
    SAAC_v_time_sub_2 = (SAAC_v_time_sub_1[-1]-SAAC_v_time_sub_1[0])/(SAAC_v_time_sub_2[-1]-SAAC_v_time_sub_2[0]) * SAAC_v_time_sub_2 + SAAC_v_time_sub_1[0] 
    SAAC_v_time_sub_3 = (SAAC_v_time_sub_1[-1]-SAAC_v_time_sub_1[0])/(SAAC_v_time_sub_3[-1]-SAAC_v_time_sub_3[0]) * SAAC_v_time_sub_3 + SAAC_v_time_sub_1[0] 
    SAAC_v_time_sub_4 = (SAAC_v_time_sub_1[-1]-SAAC_v_time_sub_1[0])/(SAAC_v_time_sub_4[-1]-SAAC_v_time_sub_4[0]) * SAAC_v_time_sub_4 + SAAC_v_time_sub_1[0]
     
    spline_hor_angle_new_sub_1 = spline_hor_angle_new[:np.where(SAAC_h_time < seperate_points_hor[0])[0][-1]]
    spline_hor_angle_new_sub_2 = spline_hor_angle_new[np.where(SAAC_h_time > seperate_points_hor[0])[0][0]:np.where(SAAC_h_time < seperate_points_hor[1])[0][-1]]
    spline_hor_angle_new_sub_3 = spline_hor_angle_new[np.where(SAAC_h_time > seperate_points_hor[1])[0][0]:np.where(SAAC_h_time < seperate_points_hor[2])[0][-1]]
    spline_hor_angle_new_sub_4 = spline_hor_angle_new[np.where(SAAC_h_time > seperate_points_hor[2])[0][0]:]
    spline_ver_angle_new_sub_1 = spline_ver_angle_new[:np.where(SAAC_v_time < seperate_points_ver[0])[0][-1]]
    spline_ver_angle_new_sub_2 = spline_ver_angle_new[np.where(SAAC_v_time > seperate_points_ver[0])[0][0]:np.where(SAAC_v_time < seperate_points_ver[1])[0][-1]]
    spline_ver_angle_new_sub_3 = spline_ver_angle_new[np.where(SAAC_v_time > seperate_points_ver[1])[0][0]:np.where(SAAC_v_time < seperate_points_ver[2])[0][-1]]
    spline_ver_angle_new_sub_4 = spline_ver_angle_new[np.where(SAAC_v_time > seperate_points_ver[2])[0][0]:]
     
    SAAC_h_sub_1 = SAAC_h[:np.where(SAAC_h_time < seperate_points_hor[0])[0][-1]]
    SAAC_h_sub_2 = SAAC_h[np.where(SAAC_h_time > seperate_points_hor[0])[0][0]:np.where(SAAC_h_time < seperate_points_hor[1])[0][-1]]
    SAAC_h_sub_3 = SAAC_h[np.where(SAAC_h_time > seperate_points_hor[1])[0][0]:np.where(SAAC_h_time < seperate_points_hor[2])[0][-1]]
    SAAC_h_sub_4 = SAAC_h[np.where(SAAC_h_time > seperate_points_hor[2])[0][0]:]
    SAAC_v_sub_1 = SAAC_v[:np.where(SAAC_v_time < seperate_points_ver[0])[0][-1]]
    SAAC_v_sub_2 = SAAC_v[np.where(SAAC_v_time > seperate_points_ver[0])[0][0]:np.where(SAAC_v_time < seperate_points_ver[1])[0][-1]]
    SAAC_v_sub_3 = SAAC_v[np.where(SAAC_v_time > seperate_points_ver[1])[0][0]:np.where(SAAC_v_time < seperate_points_ver[2])[0][-1]]
    SAAC_v_sub_4 = SAAC_v[np.where(SAAC_v_time > seperate_points_ver[2])[0][0]:]
 
    SAAC_h_time_subs = [SAAC_h_time_sub_1, SAAC_h_time_sub_2, SAAC_h_time_sub_3, SAAC_h_time_sub_4]
    SAAC_v_time_subs = [SAAC_v_time_sub_1, SAAC_v_time_sub_2, SAAC_v_time_sub_3, SAAC_v_time_sub_4]
    spline_hor_angle_new_subs = [spline_hor_angle_new_sub_1, spline_hor_angle_new_sub_2, spline_hor_angle_new_sub_3, spline_hor_angle_new_sub_4]
    spline_ver_angle_new_subs = [spline_ver_angle_new_sub_1, spline_ver_angle_new_sub_2, spline_ver_angle_new_sub_3, spline_ver_angle_new_sub_4]
    SAAC_h_subs = [SAAC_h_sub_1, SAAC_h_sub_2, SAAC_h_sub_3, SAAC_h_sub_4]
    SAAC_v_subs = [SAAC_v_sub_1, SAAC_v_sub_2, SAAC_v_sub_3, SAAC_v_sub_4]
    
    fig = plt.figure('repeatability')
    plt.gcf().set_size_inches(18,9)
    ax1 = fig.add_subplot(2, 2, 1)
    ax2 = fig.add_subplot(2, 2, 2)
    ax3 = fig.add_subplot(2, 2, 3)
    ax4 = fig.add_subplot(2, 2, 4)
    
    ax1.plot(time_set, hor_angle_avr_set, color='black', label='hor_angle')
    ax1.plot(SAAC_h_time, spline_hor_angle_new, color='red', label='interpolated')
    ax2.plot(time_set, ver_angle_avr_set, color='black', label='ver_angle')
    ax2.plot(SAAC_v_time, spline_ver_angle_new, color='red', label='interpolated')
    for k in range(4):
        ax3.plot(SAAC_h_time_subs[k], spline_hor_angle_new_subs[k], label='Round %i'%k)
        ax4.plot(SAAC_v_time_subs[k], spline_ver_angle_new_subs[k], label='Round %i'%k)
#         if len(SAAC_v_time_subs[k]) <= len(spline_ver_angle_new_subs[k]):
#             ax4.plot(SAAC_v_time_subs[k], spline_ver_angle_new_subs[k][:len(SAAC_v_time_subs[k])], label='Round %i'%k)
#         else:
#             ax4.plot(SAAC_v_time_subs[k][:len(spline_ver_angle_new_subs[k])], spline_ver_angle_new_subs[k], label='Round %i'%k)

#         ax3.plot(SAAC_h_time_subs[k], SAAC_h_subs[k], label='Round %i'%k)
#         ax4.plot(SAAC_v_time_subs[k], SAAC_v_subs[k], label='Round %i'%k)
# #         if len(SAAC_v_time_subs[k]) <= len(SAAC_v_subs[k]):
# #             ax4.plot(SAAC_v_time_subs[k], SAAC_v_subs[k][:len(SAAC_v_time_subs[k])], label='Round %i'%k)
# #         else:
# #             ax4.plot(SAAC_v_time_subs[k][:len(SAAC_v_subs[k])], SAAC_v_subs[k], label='Round %i'%k)
        
    ax1.grid()
    ax2.grid()
    ax3.grid()
    ax4.grid()
    ax1.legend()
    ax2.legend()
    ax3.legend()

    plt.show()

#     time_set_sub_1 = time_set[:np.where(time_set > time_set[-1]/4)[0][0]]
#     time_set_sub_2 = time_set[np.where(time_set >= time_set[-1]/4)[0][0]:np.where(time_set > time_set[-1]/2)[0][0]]
#     time_set_sub_2 = time_set_sub_2 - time_set_sub_2[0]
#     time_set_sub_3 = time_set[np.where(time_set >= time_set[-1]/2)[0][0]:np.where(time_set > time_set[-1]/4*3)[0][0]]
#     time_set_sub_3 = time_set_sub_3 - time_set_sub_3[0]
#     time_set_sub_4 = time_set[np.where(time_set >= time_set[-1]/4*3)[0][0]:]
#     time_set_sub_4 = time_set_sub_4 - time_set_sub_4[0]
#     
#     hor_angle_avr_set_sub_1 = hor_angle_avr_set[:np.where(time_set > time_set[-1]/4)[0][0]]
#     hor_angle_avr_set_sub_2 = hor_angle_avr_set[np.where(time_set >= time_set[-1]/4)[0][0]:np.where(time_set > time_set[-1]/2)[0][0]]
#     hor_angle_avr_set_sub_3 = hor_angle_avr_set[np.where(time_set >= time_set[-1]/2)[0][0]:np.where(time_set > time_set[-1]/4*3)[0][0]]
#     hor_angle_avr_set_sub_4 = hor_angle_avr_set[np.where(time_set >= time_set[-1]/4*3)[0][0]:]
#     
#     ver_angle_avr_set_sub_1 = ver_angle_avr_set[:np.where(time_set > time_set[-1]/4)[0][0]]
#     ver_angle_avr_set_sub_2 = ver_angle_avr_set[np.where(time_set >= time_set[-1]/4)[0][0]:np.where(time_set > time_set[-1]/2)[0][0]]
#     ver_angle_avr_set_sub_3 = ver_angle_avr_set[np.where(time_set >= time_set[-1]/2)[0][0]:np.where(time_set > time_set[-1]/4*3)[0][0]]
#     ver_angle_avr_set_sub_4 = ver_angle_avr_set[np.where(time_set >= time_set[-1]/4*3)[0][0]:]
#     
#     time_set_subs = [time_set_sub_1, time_set_sub_2, time_set_sub_3, time_set_sub_4]
#     hor_angle_avr_set_subs = [hor_angle_avr_set_sub_1, hor_angle_avr_set_sub_2, hor_angle_avr_set_sub_3, hor_angle_avr_set_sub_4]
#     ver_angle_avr_set_subs = [ver_angle_avr_set_sub_1, ver_angle_avr_set_sub_2, ver_angle_avr_set_sub_3, ver_angle_avr_set_sub_4]
    
#     fig = plt.figure('repeatability')
#     plt.gcf().set_size_inches(18,5)
#     ax1 = fig.add_subplot(1, 2, 1)
#     ax2 = fig.add_subplot(1, 2, 2)
#     for k in range(4):
#         ax1.plot(time_set_subs[k]*10, hor_angle_avr_set_subs[k], label='Round %i'%k)
#         ax2.plot(time_set_subs[k], ver_angle_avr_set_subs[k], label='Round %i'%k)
#         
#     ax1.title.set_text('Horizontal')
#     ax2.title.set_text('Vertical')
#     ax1.set_ylabel('Horizontal Tilt \ acrsec')
#     ax1.set_xlabel('Time \ s')
#     ax2.set_ylabel('Vertical Tilt \ acrsec')
#     ax2.set_xlabel('Time \ s')
#     ax1.grid(which='both', axis='both')
#     ax2.grid(which='both', axis='both')
#     ax1.legend()
#     ax2.legend()
#     plt.tight_layout()
#     
    
plt.show()
